package it.gius.processing.gjkTest;

import org.jbox2d.common.Mat22;
import org.jbox2d.common.Transform;
import org.jbox2d.common.Vec2;

import processing.core.PApplet;
import it.gius.pe.SystemCostants;
import it.gius.pe.Shape.Ellipse;
import it.gius.pe.Shape.witness.EllipsePointWitness;
import it.gius.pe.algorithm.gjk.DistanceSolution;
import it.gius.pe.algorithm.gjk.GjkEpaAlgorithm;
import it.gius.pe.algorithm.gjk.NotConvergingException;
import it.gius.pe.algorithm.gjk.shapes.GjkEllipse;
import it.gius.pe.algorithm.gjk.shapes.GjkMinkDiffShape;
import it.gius.pe.algorithm.gjk.shapes.GjkPolygon;
import it.gius.pe.mains.MainMovingTest;
import it.gius.processing.util.MyAbstractPApplet;

public class EllipsesDistanceApllet extends MyAbstractPApplet {


	private Ellipse ellipses[];
	private GjkMinkDiffShape[][] minkDiffShape;
	
	private double mediumIterationEE = 0;
	private double mediumIterationEM = 0;
	
	private long numComparisonEE = 0;
	private long numComparisonEM = 0;

	public EllipsesDistanceApllet(Ellipse[] ellipses) {
		this.ellipses = ellipses;

		minkDiffShape = new GjkMinkDiffShape[ellipses.length][ellipses.length];
	}

	public void setup(){


		cursor(PApplet.CROSS);

		size(MainMovingTest.sizeX,MainMovingTest.sizeY);
		//frameRate(15);
		colliding = new boolean[ellipses.length];
		
		if(!goStop.isGo())
			noLoop();

		
	}
	
	


	public double getMediumIterationEE() {
		return mediumIterationEE;
	}

	public double getMediumIterationEM() {
		return mediumIterationEM;
	}




	boolean colliding[];

	public void draw()
	{

		background(255);

		for(int i=0; i<colliding.length;i++)
			colliding[i] = false;

		DistanceSolution sol = new DistanceSolution();

		for(int i= 0;i < ellipses.length; i++)
		{

			for(int j=i+1; j<ellipses.length;j++)
			{

				/*try {

					if(!BoundaryBox.overlap(arrayPoly[i].getBoundaryBox(),arrayPoly[j].getBoundaryBox()))
						continue;
				} catch (BadShapeException e1) {
					e1.printStackTrace();
				}	*/

				if(minkDiffShape[i][j] == null)
					minkDiffShape[i][j] = new GjkMinkDiffShape(new GjkEllipse(ellipses[i]),new GjkEllipse(ellipses[j]));

				try {
					//GjkAlgorithm.distance(arrayPoly[i], arrayPoly[j], sol,sol.indexPoly1PointInSolution,sol.indexPoly2PointInSolution);
					GjkEpaAlgorithm.distance(minkDiffShape[i][j],SystemCostants.ORIGIN2D,sol);
					//println("sol.indexPoly1PointInSolution: " + sol.indexPoly1PointInSolution);
					//println("sol.indexPoly2PointInSolution: " + sol.indexPoly2PointInSolution);
					stroke(210,210,0);
					line(sol.p1.x,sol.p1.y,sol.p2.x,sol.p2.y);
					fill(0,0,0);
					ellipse(sol.p1.x,sol.p1.y,5,5);
					ellipse(sol.p2.x,sol.p2.y,5,5);

					if(sol.distance >= -SystemCostants.EPSILON  && sol.distance < SystemCostants.EPSILON)
					{
						colliding[i] = true;
						colliding[j] = true;
					}
					
					if(sol.penetration)
					{
						System.out.println("interpenetration between: " +i+ " and " +j);
						System.out.println("penetration depth: " + sol.penetrationDepth);
						System.out.println("penetration normal: " + sol.normalPenetration);
						System.out.println("penetration iterations: " + sol.epaIterations);
						stroke(0,0,220);
						line(sol.p1.x,sol.p1.y,sol.p1.x + 15*sol.normalPenetration.x,sol.p1.y + 15*sol.normalPenetration.y);
					}
					
					//println("iteration for ellipse ellipse: " + sol.iterationCount);
					
					mediumIterationEE = mediumIterationEE*numComparisonEE + sol.iterationCount;
					numComparisonEE++;
					mediumIterationEE = mediumIterationEE/numComparisonEE;
					//System.out.println("mediumIterationEE: " + mediumIterationEE);
					//System.out.println("numComparisonEE: " + numComparisonEE);
					
				} catch (NotConvergingException e) {
					e.printStackTrace();
					goStop.setGo(false);
				}


			}

			fill(0,0,210);
			stroke(10);
			/*println("sol.distance: " + sol.distance);
			println("center firt poly: " + arrayPoly[0].getCenter());
			println("center second poly: " + arrayPoly[1].getCenter());*/
			if(colliding[i])
			{
				//if(sol.distance ==0)
				fill(210,0,0);
				stroke(210,0,0);
			}


			noFill();
			drawEllipse(ellipses[i]);
			
			EllipsePointWitness witness = new EllipsePointWitness(ellipses[i].getA());
			witness.eX = ellipses[i].getA();
			witness.eY = 0;
			
			Vec2 tempPoint = new Vec2();
			tempPoint.x = witness.eX;
			tempPoint.y = witness.eY;
			
			Mat22 mat = new Mat22();
			mat.set(ellipses[i].angle);
			Transform t = new Transform(ellipses[i].centerPosition, mat);
			Transform.mulToOut(t, new Vec2(witness.eX,witness.eY), tempPoint);
			
			ellipse(tempPoint.x,tempPoint.y,4,4);
			
			
			
			Vec2 tangentResult = new Vec2();
			ellipses[i].ccTangent(witness, tangentResult);
			
			tangentResult.normalize();
			
			line(ellipses[i].centerPosition.x,ellipses[i].centerPosition.y,
					ellipses[i].centerPosition.x +15*tangentResult.x,ellipses[i].centerPosition.y + 15*tangentResult.y);

			Vec2 normalResult = new Vec2();
			ellipses[i].normal(witness, normalResult);
			
			normalResult.normalize();
			
			line(ellipses[i].centerPosition.x,ellipses[i].centerPosition.y,
					ellipses[i].centerPosition.x +30*normalResult.x,ellipses[i].centerPosition.y + 30*normalResult.y);

		}

	}

	private void drawEllipse(Ellipse ellipse)
	{
		pushMatrix();
		translate(ellipse.centerPosition.x, ellipse.centerPosition.y);
		rotate(ellipse.angle);

		ellipse(0, 0, 2*ellipse.getA(), 2*ellipse.getB());

		popMatrix();
	}


	private boolean dragging = false;
	private int indexDragging = -1;

	@Override
	public void mousePressed() {

		DistanceSolution sol = new DistanceSolution();
		for(int i=0; i < ellipses.length;i++)
			try {
				GjkEpaAlgorithm.distance(new GjkEllipse(ellipses[i]), new Vec2(mouseX,mouseY), sol);

				if(sol.distance <= 0)
				{
					dragging = true;
					indexDragging = i;
					if(!goStop.isGo())
						goStop.setGo(true);

					return;
				}

				//println("iteration for ellipse mouse: " + sol.iterationCount);
				
				mediumIterationEM = mediumIterationEM*numComparisonEM + sol.iterationCount;
				numComparisonEM++;
				mediumIterationEM = mediumIterationEM/numComparisonEM;
				
				//System.out.println("mediumIterationEM: " + mediumIterationEM);
			} catch (NotConvergingException e) {
				e.printStackTrace();
				println("mouseX: " + mouseX);
				println("mouseY: " + mouseY);
			}
	}
	
	
	@Override
	public void mouseDragged() {
		if(dragging)
		{
				ellipses[indexDragging].centerPosition.set(mouseX,mouseY);
		}
	}

	@Override
	public void mouseReleased() {
		dragging = false;
		indexDragging = -1;
		goStop.setGo(false);
	}
}
